#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy, sys
from time import sleep
from sg805_driver.msg import FastPoint, ExtraFeatures




class FastPointDemo:

    fastPointMsg = FastPoint()
    extraMsg = ExtraFeatures()

    def __init__(self):
        rospy.init_node('fastPointCommander', anonymous = False )
        self.fastPointPub = rospy.Publisher("/sg805/fastPoint", FastPoint, queue_size=10)
        self.extraPub = rospy.Publisher("/sg805/extraFeatures", ExtraFeatures, queue_size=10)

        self.fastPointMsg.Mode = 0



        sleep(2)    # 建立连接之后不能马上发送数据
        print("下降")
        self.fastPointMsg.Position.clear()
        self.fastPointMsg.duration = 4000
        self.fastPointMsg.Position.append(-1024)
        self.fastPointMsg.Position.append(36864)        # 8192
        self.fastPointMsg.Position.append(-2048)
        self.fastPointMsg.Position.append(-52224)
        self.fastPointMsg.Position.append(6144)    # -8192
        self.fastPointMsg.Position.append(-27136)
        #self.fastPointMsg.Position.append(27136)
        self.fastPointPub.publish(self.fastPointMsg)
        sleep(5)
        print("抓取")
        self.grasp()

        
        
        print("悬挂")
        self.fastPointMsg.Position.clear()
        self.fastPointMsg.duration = 4000
        self.fastPointMsg.Position.append(-1024)        # 8192
        self.fastPointMsg.Position.append(22763)
        self.fastPointMsg.Position.append(1489)
        self.fastPointMsg.Position.append(-52224)
        self.fastPointMsg.Position.append(11374)   # -13422
        self.fastPointMsg.Position.append(-27136)
        #self.fastPointMsg.Position.append(27136) 
        self.fastPointPub.publish(self.fastPointMsg)








        sleep(5)
        print("释放")
        self.release()


    # 释放
    def release(self):
        self.rads = 25
        self.sleepDuration = int(self.rads*0.16) + 1
        self.extraMsg.Tag = 6
        self.extraMsg.PSC = 499
        self.extraMsg.ARR = 9
        self.extraMsg.CCR1 = 5
        self.extraMsg.PluseCount = 6400 * self.rads        # 0.16秒/转
        self.extraPub.publish(self.extraMsg)
        sleep(self.sleepDuration)


    # 夹取
    def grasp(self):
        self.rads = 25
        self.sleepDuration = int(self.rads*0.16) + 1
        self.extraMsg.Tag = 6
        self.extraMsg.PSC = 499
        self.extraMsg.ARR = 9
        self.extraMsg.CCR1 = 5
        self.extraMsg.PluseCount = -6400 * self.rads   # 0.16秒/转
        self.extraPub.publish(self.extraMsg)
        sleep(self.sleepDuration)


if __name__ == '__main__':
    try:
        handle = FastPointDemo()
    except rospy.ROSInterruptException:
        pass